import rclpy  
from rclpy.node import Node  
from nav_msgs.msg import Odometry   
from geometry_msgs.msg import Twist  
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Imu  
import numpy as np  
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from rcl_interfaces.msg import SetParametersResult
from ai_msgs.msg import PerceptionTargets
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
from std_msgs.msg import String
import math
import datetime
import time
from pyzbar.pyzbar import decode

from sensor_msgs.msg import Image
import cv2

from threading import Thread
import keyboard

import sys
import tty
import termios

import os
from openai import OpenAI
import base64
import json


class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node')  # 初始化节点名称

        self.last_log_time_vel_callback = self.get_clock().now()  # 记录上次日志时间
        self.last_log_time_navagation_0 = self.get_clock().now()
        self.last_log_time_navagation_3 = self.get_clock().now()
        self.task3_info = 0
        self.qrcode_info = 0
        self.model_path = './model/fcos_test2.0.bin'
        # 用于判断是否允许开始导航，1为开始，0为停止
        self.flag = 1

        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航  
        #避障导航也就是添加了简单的避障，2和3是将目标点给替代掉  
        self.state = 0

        #目标点切换（任务一任务三启动标志）
        self.task = 0

        #任务中途切换目标位置
        #self.change = 0

        #0为默认，1为启动大模型调用
        self.vlm_flag = 0
        #0为默认，1为启动cv检测
        self.opencv_flag = 0


        # 创建IMU数据订阅
        self.subscription_imu_data = self.create_subscription(  
            Imu,  
            '/imu/data_raw',  
            self.imu_callback,  
            3
        )
        # 创建目标位置订阅，主要用于订阅目标位置，这个的y的位置偏差较大
        self.subscription_robotvel = self.create_subscription(
            Data,  
            '/robotvel',
            self.robotvel_callback,
            3
        )
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10)     
        # 创建订阅者，订阅话题 /image
        self.subscription = self.create_subscription(
            Image,
            '/image',
            self.listener_callback,
            3)
        self.subscription_vlm = self.create_subscription(  
            Int32,  
            '/vlm',  
            self.vlm_callback,  
            3)
        
        #创建订阅者，订阅话题 hobot_dnn_detection 话题   
        self.subscription_fcos = self.create_subscription( 
            PerceptionTargets, 
            'hobot_dnn_detection',  
            self.fcos_callback, 
            3)   

        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)  
        # 创建发布者，发布到 /sign_switch 话题
        self.sign_switch_pub = self.create_publisher(Sign, 'sign_switch', 10)      # 发布二维码的信息
        #创建新的发布者，用于发布避障信息
        self.cone_publisher = self.create_publisher(Int32MultiArray, '/cone', 10)  # 创建新的发布者，用于发布x和y坐标
        #创建新的发布者，用于发布park信息
        self.park_publisher = self.create_publisher(Int32MultiArray, '/park', 10)  # 创建新的发布者，用于发布x和y坐标   
        #创建发布者，发布vlm识别结果
        self.vlm_msg_publisher = self.create_publisher(String, '/vlm_msg', 10)
        #创建发布者，发布二维码识别结果
        self.qrcode_publisher = self.create_publisher(String, '/qrcode', 10)

        #图像识别参数
        self.up = 120
        self.left = 50
        self.area = 1200    
        self.right_park = 640  #580
        self.up_park = 270    
        self.left_cone = 0
        self.right_cone = 0

        # 初始化状态变量
        self.angular =  0
        self.velocity = 0 #阿克曼小车的速度只有x方向
        self.position = np.array([0.0, 0.0])  # 当前位置信息

        self.target_position = np.array([0.0, 0.0])  # 目标位置        
        self.target_angular = 0.0  # 目标角度  
        
        #由于识别到停车场和二维码后会前往，所以这里设置了相对位置
        self.relativ_target_position = np.array([0.0, 0.0])  # 相对目标位置
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        
        #锥桶的相对坐标在这里，分别是xy,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        self.safety_zhuitong_height = 120
        self.safety_zhuitong_width = 140
        self.zhuitong_position = np.array([0.0, 0.0])

        #这里使用了加速度计校准
        self.sample_size =100
        self.angular_velocity_sample = []
        self.bias_correction = 0
        self.imu_z = 0
        self.robotvel_last_time = self.imu_last_time = self.get_clock().now()   
        self.task1_time = 0
        self.task3_time = 0
    
        #参数设置
        self.declare_parameter ('kp',0.04)
        self.declare_parameter ('stop_distance',0.1) 
        self.declare_parameter ('vx',0.5)
        self.declare_parameter ('vx_3',0.25)
        self.declare_parameter ('safe_distance',0.8) 
        self.declare_parameter ('turn_time_1',1.0)
        self.declare_parameter ('turn_time_2',1.2)
        self.declare_parameter ('turn_v',0.4)
        self.declare_parameter ('turn_w',0.01)
        self.declare_parameter ('turn_error',0)
        self.declare_parameter ('zhuitongyuzhi',300) 
        self.declare_parameter ('radius_cone',0.3)

        self.kp = self.get_parameter("kp").value
        self.stop_distance = self.get_parameter("stop_distance").value
        self.vx = self.get_parameter("vx").value
        self.vx_3 = self.get_parameter("vx_3").value
        self.safe_distance = self.get_parameter("safe_distance").value
        self.turn_time_1 = self.get_parameter("turn_time_1").value
        self.turn_time_2 = self.get_parameter("turn_time_2").value
        self.turn_v = self.get_parameter("turn_v").value
        self.turn_w = self.get_parameter("turn_w").value   
        self.turn_error = self.get_parameter("turn_error").value  
        self.zhuitongyuzhi = self.get_parameter("zhuitongyuzhi").value
        self.radius_cone = self.get_parameter("radius_cone").value

        #参数回调函数
        self.add_on_set_parameters_callback(self.parameters_callback)

        # 透视变换矩阵
        '''
        self.matrix = np.array([
            [-4.78185634e-02, -4.35936013e-01, 1.17525583e+02],
            [5.34996953e-03, -8.68712398e-01, 2.17611259e+02],
            [2.24575710e-05, -4.29432464e-03, 1.00000000e+00]
        ])
        '''
    def vlm_callback(self, msg):  #接受按键信息，并设置vlm_flag为1，启动大模型，可设置cv检测
        self.vlm = msg.data
        if self.vlm == 1:
            self.vlm_flag = 1
        if self.vlm == 2:
            self.opencv_flag = 1
        if self.vlm == 3:
            self.opencv_flag = 0

    def qwen_vl(self,frame):    #调用qwen API

        client = OpenAI(
            # 若没有配置环境变量，请用百炼API Key将下行替换为：api_key="sk-xxx",
            api_key=os.getenv("DASHSCOPE_API_KEY"),
            base_url="https://dashscope.aliyuncs.com/compatible-mode/v1",
        )
        #  Base64 编码格式
        # def encode_image(image_path):
        #     with open(image_path, "rb") as image_file:
        #         return base64.b64encode(image_file.read()).decode("utf-8")

        """将 OpenCV 读取的帧转换为 Base64"""
        _, buffer = cv2.imencode(".png", frame)  # 将 frame 转换为 PNG 格式的字节流
        base64_image = base64.b64encode(buffer).decode("utf-8")
            
        completion = client.chat.completions.create(
            model="qwen-vl-plus",  # 此处以qwen-vl-plus为例，可按需更换模型名称。模型列表：https://help.aliyun.com/zh/model-studio/getting-started/models
            messages=[{"role": "user","content": [
                    {"type": "text","text": "中文描述"},
                    {"type": "image_url",
                    #"image_url": {"url": "https://dashscope.oss-cn-beijing.aliyuncs.com/images/dog_and_girl.jpeg"}}
                    "image_url": {"url": f"data:image/png;base64,{base64_image}"}}
                    ]}],
            max_tokens=30,
            )
        response_str = completion.model_dump_json()
        response_dict = json.loads(response_str)
        content = response_dict["choices"][0]["message"]["content"]

        return content

    def fcos_callback(self, msg):
        for num, target in enumerate(msg.targets):
            class_name = target.rois[0].type.strip()
            if target.rois:

                # 获取第一个 ROI
                roi = target.rois[0].rect

                # 获取框参数
                x_offset = roi.x_offset
                y_offset = roi.y_offset
                height = roi.height
                width = roi.width
                confidence =target.rois[0].confidence
                
            if class_name == '2obstcle': 
                #底边中点坐标
                x = int(x_offset + 0.5 * width)
                y = y_offset + height
                return x,y,width,height

            else:
                x = int(x_offset + 0.5 * width)
                y = int(y_offset + 0.5 * height)
                return x,y,width,height,confidence,class_name

    def listener_callback(self, msg):  #image话题的回调函数，使用cv获取物体参数，设置小车模式；接受vlm_flag开启大模型图生文

        #将图像数据转换为OpenCV格式
        np_arr = np.frombuffer(msg.data, np.uint8)
        cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        if(self.vlm_flag == 1):
            vlm_msg = String()
            vlm_msg.data = self.qwen_vl(frame=cv_image)
            self.vlm_msg_publisher.publish(vlm_msg)
            self.get_logger().info(f"大模型识别结果为: {vlm_msg.data}")
            self.vlm_flag = 0

        if(self.opencv_flag == 1):
            #获取锥桶的底边中点与宽高的坐标（x,y,w,h)
            zhuitong_position = self.process_frame(cv_image)
            #把坐标发布出去方便调试
            cone_msg = Int32MultiArray()
            cone_msg.data = zhuitong_position
            self.cone_publisher.publish(cone_msg)

            #获取P点的中点与宽高(x,y,w,h)
            park_position = self.park_frame(cv_image)
            #把坐标发布出去方便调试
            park_msg = Int32MultiArray()
            park_msg.data =  park_position
            self.park_publisher.publish(park_msg)


        if(self.task == 1): 
            time = self.get_clock().now()
            dt_1 = (time - self.task1_time).nanoseconds * 1e-9

            x,y,width,height,confidence,class_name = self.fcos_callback()
            #获取锥桶的底边中点与宽高的坐标（x,y,w,h)
            if class_name == '2obstcle':
                self.zhuitong_relativ_position = x,y,width,height
                #把坐标发布出去方便调试
                cone_msg = Int32MultiArray()
                cone_msg.data = self.zhuitong_relativ_position
                self.cone_publisher.publish(cone_msg)
            #这里设置一下状态
            if (self.zhuitong_relativ_position[0] != 0  and self.zhuitong_relativ_position[1] > self.zhuitongyuzhi):
                self.state = 1#避障导航模式
                # self.left_cone = int(self.zhuitong_relativ_position[0] - self.zhuitong_relativ_position[2]) - self.turn_error   #校正左转点
                # self.right_cone = int(self.zhuitong_relativ_position[0] + self.zhuitong_relativ_position[2]) + self.turn_error   #校正右转点
            else :
                self.state = 0 #普通导航模式

            if dt_1 > 3:
                if self.qrcode_info == 0:
                    self.get_logger().info(f"开始识别二维码") #确保只打印一次
                    self.qrcode_info = 1
                #image_roi = cv_image[0:480,0:640] #截取图片，先高后宽 [50:450,0:640]
                # 选择解码方法，注释掉不需要的部分以使用另一种方法
                self.decode_qr_code_pyzbar(cv_image)  # 使用 pyzbar 解码

        if(self.task == 3):
            time = self.get_clock().now()
            dt = (time - self.task3_time).nanoseconds * 1e-9

            x,y,width,height,confidence,class_name = self.fcos_callback()
            #为防止干扰，4秒内识别锥桶，不识别停车点；4秒后识别停车点，不识别锥桶
            if class_name == '2obstcle':
                self.zhuitong_relativ_position = x,y,width,height
                #把坐标发布出去方便调试
                cone_msg = Int32MultiArray()
                cone_msg.data = self.zhuitong_relativ_position
                self.cone_publisher.publish(cone_msg)

            if class_name == '3p':
                #获取P点的中点与宽高(x,y,w,h)
                self.park_relativ_position = x,y,width,height
                #把坐标发布出去方便调试
                park_msg = Int32MultiArray()
                park_msg.data = self.park_relativ_position
                self.park_publisher.publish(park_msg)

            #这里设置一下状态
            if (self.zhuitong_relativ_position[0] != 0  and self.zhuitong_relativ_position[1] > self.zhuitongyuzhi):
                self.state = 1
                # self.left_cone = int(self.zhuitong_relativ_position[0] - self.zhuitong_relativ_position[2]) - self.turn_error   #校正左转点
                # self.right_cone = int(self.zhuitong_relativ_position[0] + self.zhuitong_relativ_position[2]) + self.turn_error   #校正右转点
            elif(self.park_relativ_position[0] != 0):
                self.state = 3 
            else:
                self.state = 0
           
    def sign4return_callback(self,msg): #sign4return话题的回调函数，在foxglove上显示

    # 创建 Sign 消息实例
        if msg.data == 6:
            self.flag = 1
            self.state = 0
            self.task = 3
            self.task3_info = 1

            # 定位任务三中途目标点
            self.target_position[0] = 0  # 目标x和y坐标
            self.target_position[1] = 0
            self.target_angular =  0   
            self.get_logger().info(f"任务三")
            self.get_logger().info(f"结束遥感")

            self.task3_time = self.get_clock().now()

            self.position[0] = 2.5
            self.position[1] = 2.2
            self.angular = -np.pi/2

            ##这里会重置里程计到(2,2)

        if msg.data == -1:
            self.flag = 1
            self.state = 0
            self.task = 1
            #前往二维码区
            self.target_position[0] = 4.6  # 目标x和y坐标
            self.target_position[1] = 1.8
            
            self.target_angular =  0             
            self.get_logger().info(f"任务一")
            self.get_logger().info(f"重置里程计")

            self.task1_time = self.get_clock().now()

            self.angular = 0
            self.position[0] = 0.68
            self.position[1] = 0.2
            ##这里重置了里程计，将他的位置初始化成（0.5，0.2）
            
        if msg.data == 5:
            self.flag = 0
            self.task = 2
            self.get_logger().info(f"任务二")
            self.get_logger().info(f"C区进行遥测")

        return
        
    def imu_callback(self, msg):  # 回调函数，接收IMU数据，并调用navigation_control进行任务一，任务三的控制

        angular_velocity = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])  
        
        if len(self.angular_velocity_sample) <= self.sample_size:
            self.angular_velocity_sample.append(angular_velocity[2])   
        if len(self.angular_velocity_sample) == self.sample_size:
                # 计算偏移量
            self.bias_correction = np.mean(self.angular_velocity_sample)
            self.get_logger().info('偏移量: {}'.format(self.bias_correction))

        if  len(self.angular_velocity_sample) > self.sample_size: 
             self.imu_z = angular_velocity[2]-self.bias_correction

        '''
            current_time = self.get_clock().now()
            dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
            self.imu_last_time = current_time

            self.angular += (angular_velocity[2]-self.bias_correction) * dt  # 更新角度
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    
        '''

        if self.task == 1 :
            self.navigation_control()
        if self.task == 3 :
            distance = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
            #这里小车常常进不去if，因为小车速度快，拐角小，线速度角速度耦合，所以速度大的时候总是在目标点周围却没有满足if条件，就会转圈圈
            #适当调节self.stop_distance与self.vx
            if distance <= self.stop_distance:
                self.target_position[0] = -0.1  # 这里小车位置不太准，不一定设定成0.0,0.0，可以设置成参数具体调整即可
                self.target_position[1] = -0.1
            self.navigation_control()
            if self.task3_info == 1:
                self.get_logger().info('任务三目标位置位置: x={:.6f}, y={:.6f},state={}'.format(self.target_position[0], self.target_position[1],self.state))
                self.task3_info = 0
            
    def robotvel_callback(self , msg):  #回调函数，接收小车速度，并更新小车位置，角度(当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{})
        x_speed = msg.x
        y_speed = msg.y
        z_speed = msg.z
        current_time = self.get_clock().now()
        dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
        self.imu_last_time = current_time

        
        self.position[0]+=1.13*( x_speed * math.cos(self.angular) ) * dt
        self.position[1]+=1.03*( x_speed * math.sin(self.angular) ) * dt
        

        if (x_speed != 0):
            self.angular = self.imu_z * dt + self.angular
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    

        # 计算时间差
        elapsed_time = (current_time - self.last_log_time_vel_callback).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info(
                '当前位置: x={:.6f}, y={:.6f}, 角度={:.6f}, 状态{}, 任务{}'.format(
                    self.position[0], self.position[1], self.angular, self.state, self.task
                )
            )
            self.last_log_time_vel_callback = current_time  # 更新日志时间
        #self.get_logger().info('当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{}'.format(self.position[0], self.position[1], self.angular,self.state,self.task))

    def navigation_control(self): #总导航控制逻辑
        erro = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
        if ( erro > self.stop_distance):
            if self.flag == 1:
                if self.state == 0 :
                    self.navigation_0(self.target_position[0],self.target_position[1])
                elif self.state == 1 :
                    self.navigation_1()
                elif self.state == 2 :
                    self.navigation_2()
                elif self.state == 3:
                    #self.navigation_0(self.target_position[0],self.target_position[1])
                    self.navigation_3()
            else:
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                # 发布 Twist 消息
                self.cmd_pub.publish(twist_msg)     

        elif( erro < self.stop_distance):
            cmd_msg = Twist()  
            cmd_msg.linear.x = 0.0  # 设置线性速度  
            cmd_msg.angular.z = 0.0  # 设置角速度  
            self.cmd_pub.publish(cmd_msg)  # 发布控制指令
    
    def navigation_0(self,target_x,target_y): #state=0 无避障简单惯导
                    
        target_angle = np.arctan2((target_y - self.position[1]) , (target_x - self.position[0]))
        angular =  target_angle - self.angular  # 计算角的方向的插值  

        # 构造控制指令  
        cmd_msg = Twist()  
        '''
        if self.task == 3:
            cmd_msg.linear.x = self.vx_3  # 设置线性速度  
            cmd_msg.angular.z = 1 * angular  # 设置角速度  
        
        if self.task == 1:
        '''
        cmd_msg.linear.x = self.vx
        cmd_msg.angular.z = 2 * angular  # 设置角速度  
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令  

        # 打印当前的角度和转速（调试用）  
        # 计算时间差
        current_time_navagation_0 = self.get_clock().now()
        elapsed_time = (current_time_navagation_0 - self.last_log_time_navagation_0).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('目标角度: target_angle={:.6f},'.format(target_angle))
            self.get_logger().info('当前角速度: angular={:.6f},'.format(cmd_msg.angular.z))
            self.last_log_time_navagation_0 = current_time_navagation_0 
  
    def navigation_1(self): #state=1 避障惯导

        self.get_logger().info(f"识别到锥桶")

        #锥桶在右，向左避障
        if(self.zhuitong_relativ_position[0] > 320 and self.zhuitong_relativ_position[0] < 500 ):
            if(self.task == 1):
                    self.get_logger().info(f"任务1向左避障")
                    self.navigation_0(self.position[0]+0.2,self.position[1] + 2.0)
                    #time.sleep(0.05)
            elif(self.task == 3):
                    self.navigation_0(self.position[0]-0.2,self.position[1]-3.0)
                    #self.action_cone()
                    self.get_logger().info(f"任务3向左避障")
                    #time.sleep(0.05)
        #锥桶在左，向右避障
        elif(self.zhuitong_relativ_position[0] < 320 and self.zhuitong_relativ_position[0] >150 ):
            if(self.task == 1):
                     self.navigation_0(self.position[0]+0.2,self.position[1] - 2.0)
                     self.get_logger().info(f"任务1向右避障")
                     #time.sleep(0.05)
            elif(self.task == 3):
                    #self.action_cone()
                    self.navigation_0(self.position[0]-0.2,self.position[1]+ 5.0)   
                    # cmd_msg = Twist()  
                    # cmd_msg.linear.x = self.vx
                    # cmd_msg.angular.z = - 2.0
                    # self.cmd_pub.publish(cmd_msg)  # 发布控制指令  
                    self.get_logger().info(f"任务3向右避障")
                    #time.sleep(0.05)
        else:
            self.navigation_0(self.target_position[0],self.target_position[1])

    def navigation_2(self): #state=2，二维码导航控制 实际上并未使用
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度
        self.get_logger().info('前往二维码角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

    def navigation_3(self): #state=3，cv识别到停车点，进行导航，
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

        current_time_navagation_3 = self.get_clock().now()
        elapsed_time = (current_time_navagation_3 - self.last_log_time_navagation_3).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('返回P点速度: linear.x={:.6f},'.format(cmd_msg.linear.x ))
            self.get_logger().info('返回P点角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
            self.last_log_time_navagation_3 = current_time_navagation_3 

    def action_cone(self): #另一种避障逻辑，并未使用
        twist_msg = Twist()
        if self.zhuitong_relativ_position[0] >= 320:
            if (320-self.left_cone ) > 0:
                twist_msg.angular.z = self.turn_w * (320-self.left_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.left_cone}")
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg)    
                return                  
            else :
                self.navigation_0(self.target_position[0],self.target_position[1])
                
        if self.zhuitong_relativ_position[0] < 320:
            if (320-self.right_cone ) < 0:
                twist_msg.angular.z = self.turn_w * (320-self.right_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.right_cone}")  
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg) 
                return
            else :
                self.navigation_0(self.target_position[0],self.target_position[1]) 
    
    def perspective_point(self,x,y): #透视变换函数，输入yolo锥桶底边中点，输出相对小车实际位置，右正，左负
        homogeneous_point = np.array([x, y, 1.0], dtype=np.float32)
        transformed_point = np.dot(self.matrix, homogeneous_point)
        transformed_point /= transformed_point[2]
        #if int(round(transformed_point[0])) > 200 or int(round(transformed_point[1])) > 200 or int(round(transformed_point[0])) < 0 or int(round(transformed_point[1])) < 0:
        #    return None
        #else:
        location =(-1.0 * (0.01*(transformed_point[0] - 100.0)), 0.01*(200.0 - transformed_point[1]))
        return location
        #参数回调函数,回调使用       

    def decode_qr_code_pyzbar(self, image):  #识别到二维码结果后停车，进入任务二
        # 使用 pyzbar 进行二维码解码
        #gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        image = cv2.resize(image,(320,240)) #处理更快，但是精度更低
        decoded_objects = decode(image)
        
        # 遍历结果并判断是否是顺时针或逆时针二维码
        for obj in decoded_objects:
            qr_code_data = obj.data.decode('utf-8')
            if qr_code_data in ["ClockWise", "AntiClockWise"]:
                self.publish_sign_message(qr_code_data)
                self.flag  = 0 
                self.task = 2

                #停车
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                self.cmd_pub.publish(twist_msg)
                self.get_logger().info(f"已复位")

                return  # 成功识别并发布后立即返回，避免重复处理
    
    def publish_sign_message(self, qr_code_data): #发布二维码的信息
        # 创建并发布 Sign 消息
        sign_msg = Sign()
        sign_msg.sign_data = 3 if qr_code_data == "ClockWise" else 4
        self.sign_switch_pub.publish(sign_msg)
        qrcode_msg = String()
        qrcode_msg.data =  f"二维码的识别结果为: {qr_code_data}"
        self.qrcode_publisher.publish(qrcode_msg)
        self.get_logger().info(f"已发布二维码信息: {qr_code_data}")

    def process_frame(self,frame):  #返回锥桶的x0,y0,w0,h0，底边中点与宽高
        x0 = 0
        y0 = 0
        w0 = 0
        h0 = 0
        im0 = frame[self.up:450, self.left:600]
        hsv_img = cv2.cvtColor(im0, cv2.COLOR_BGR2HSV)

        # Define range of target color in HSV
        lower_hsv1 = np.array([0, 0, 0])
        lower_hsv2 = np.array([82, 255, 255])
        upper_hsv1 = np.array([0, 0, 0])
        upper_hsv2 = np.array([255, 90, 255])

        # Threshold the HSV image
        imgThreshLow = cv2.inRange(hsv_img, lower_hsv1, lower_hsv2)
        imgThreshHigh = cv2.inRange(hsv_img, upper_hsv1, upper_hsv2)
        threshed_img = cv2.bitwise_or(imgThreshLow, imgThreshHigh)

        # Find contours in the black regions of smoothed_img
        contours, hierarchy = cv2.findContours(255 - threshed_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)


        # Initialize variables to store the largest contour
        max_area = 0
        largest_rect = None
        largest_cnt = None

        # Identify the largest contour and store its bounding rectangle
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > self.area:  # Filter small contours if needed
                x, y, w, h = cv2.boundingRect(cnt)
                if area > max_area:
                    max_area = area
                    largest_rect = (x, y, w, h)
                    largest_cnt = cnt

        # Draw bounding rectangles and centroids
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > self.area:
                x, y, w, h = cv2.boundingRect(cnt)

                # Draw the largest rectangle in red, others in green
                if (x, y, w, h) == largest_rect:
                    color = (0, 0, 255)  # Red for the largest rectangle
                    x0 = int(x + self.left + w / 2)
                    y0 = int(y + self.up + h )
                    w0 = int(w)
                    h0 = int(h)         
     
        return x0,y0,w0,h0
    
    def park_frame(self,frame): #返回P点的x0,y0,w0,h0 中心点与宽，高
            x0 = 0
            y0 = 0
            w0 = 0
            h0 = 0
            im0 = frame[self.up_park:450, 0:self.right_park]#增大右边界就要减小lower_hsv2第二个阈值
            hsv_img = cv2.cvtColor(im0, cv2.COLOR_BGR2HSV)

            # Define range of target color in HSV
            lower_hsv1 = np.array([0, 0, 0])
            lower_hsv2 = np.array([47, 73, 255])
            upper_hsv1 = np.array([0, 0, 0])
            upper_hsv2 = np.array([255, 50, 255])#60

            # Threshold the HSV image，阈值内为白色（255）
            imgThreshLow = cv2.inRange(hsv_img, lower_hsv1, lower_hsv2)
            imgThreshHigh = cv2.inRange(hsv_img, upper_hsv1, upper_hsv2)
            threshed_img = cv2.bitwise_and(imgThreshLow, 255-imgThreshHigh)

            # Find contours in the black regions of smoothed_img
            contours, hierarchy = cv2.findContours(threshed_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

            # Initialize variables to store the largest contour
            max_area = 0
            largest_rect = None
            largest_cnt = None

            # Identify the largest contour and store its bounding rectangle
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:  # Filter small contours if needed
                    x, y, w, h = cv2.boundingRect(cnt)
                    if area > max_area:
                        max_area = area
                        largest_rect = (x, y, w, h)
                        largest_cnt = cnt

            # Draw bounding rectangles and centroids
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:
                    x, y, w, h = cv2.boundingRect(cnt)

                    # Draw the largest rectangle in red, others in green
                    if (x, y, w, h) == largest_rect:

                        x0 = int(x + w / 2)
                        y0 = int(y + self.up_park + h / 2)
                        w0 = int(w)
                        h0 = int(h)
                
            return  x0,y0,w0,h0 
    
    def parameters_callback(self,paramters): #参数设置回调函数
        for paramter in paramters:
            self.get_logger().info("Parameter: {} - Value: {}".format(paramter.name, paramter.value))
            if paramter.name == "kp":
                self.kp = paramter.value
            elif paramter.name == "vx":
                self.vx = paramter.value   
            elif paramter.name == "vx_3":
                self.vx_3 = paramter.value   
            elif paramter.name == "stop_distance":
                self.stop_distance = paramter.value  
            elif paramter.name == "safe_distance":
                self.safe_distance = paramter.value
            elif paramter.name == "turn_time_1":
                self.turn_time_1 = paramter.value  
            elif paramter.name == "turn_time_2":
                self.turn_time_2 = paramter.value
            elif paramter.name == "turn_v":
                self.turn_v = paramter.value  
            elif paramter.name == "turn_w":
                self.turn_w = paramter.value 
            elif paramter.name == "turn_error":
                self.error = paramter.value    
            elif paramter.name == "zhuitongyuzhi":
                self.zhuitongyuzhi = paramter.value  
            elif paramter.name == "radius_cone":
                self.radius_cone = paramter.value  
     
        return SetParametersResult(successful=True)

def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数




